#ifndef COMPUTECALIBRATIONRECT_HPP
#define COMPUTECALIBRATIONRECT_HPP

#include "DataStructures.h"

#include <opencv2/core/core.hpp>
#include <vector>

namespace idch {

    /**
      * Computes intrinsic and extrinsic calibration using rectangular calibration target.
      */
    class ComputeCalibrationRect
    {
    public:
        ComputeCalibrationRect( double targetWidth , double targetHeight ,
                                int imgWidth , int imgHeight , int numTrials , double inlierTol );
        ~ComputeCalibrationRect();

        void process( const std::vector<CalibrationLocation>& obs );

        double getError();

        const StereoCalib& getCalib();

        const std::vector<CalibrationLocation>& getUsed();

    private:

        // calculates the calibration and returns the repojection error
        double calibrate( const std::vector<CalibrationLocation>& obs , StereoCalib &calib , bool useEstimate );

        int countInliers( const std::vector<CalibrationLocation>& obs , const StereoCalib &calib );
        void findInliers( const std::vector<CalibrationLocation>& obs , const StereoCalib &calib ,
                          std::vector<CalibrationLocation>& inliers );
        void pruneWorst( const std::vector<CalibrationLocation>& obs , const StereoCalib &calib ,
                        std::vector<CalibrationLocation>& best , double frac );

        double computeError( const CalibrationLocation& obs , const StereoCalib &calib );

        double reprojectionError( const std::vector<cv::Point2f> &pixels ,
                                  const cv::Mat& R , const cv::Mat& T ,
                                  const cv::Mat& K, const cv::Mat& coef );

        // Stores the target location in its own coordinate system
        std::vector<cv::Point3f> targetDesc;

        // tolerance for what defines an inlier
        double inlierTol;
        // how many times RANSAC will run
        int numTrials;
        // how many targets will it sample in each cycle
        int sampleSize;

        // image dimension in pixels
        int imgWidth;
        int imgHeight;

        // observations that were used
        std::vector<CalibrationLocation> used;
        double errorUsed;

        // camera calibration for both cameras
        StereoCalib calib;
    };

}


#endif // COMPUTECALIBRATIONRECT_HPP
